// 初始化PTP头并生成链路延时报文以及发送链路延时报文
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include <cstring>
#include <netinet/in.h> // 与 Internet 协议族相关的数据结构
#include "ptp.h"
#include "tsmp.h"
#include <cmath>

const uint32_t MAX_PKT_LEN = 2048;
uint64_t clock_identity = 0x00;
uint8_t buffer_[MAX_PKT_LEN];
uint8_t src_mac[6];
uint8_t dst_mac[6];

Port_delay_state link_delay_state;

Port_delay port_delay; // 转时间的 ptp时间戳转为64位
uint64_t BufferToTimeStamp(const uint8_t *buffer)
{
    uint32_t s_high = 0;
    uint16_t s_low = 0;

    uint32_t nanosecond = 0;

    memcpy((uint8_t *)&s_high, buffer, 4);
    memcpy((uint8_t *)&s_low, buffer + 4, 2);
    memcpy((uint8_t *)&nanosecond, buffer + 6, 4);

    uint64_t t =
        (((uint64_t)ntohl((uint64_t)s_high) << 16) | ntohs((uint64_t)s_low)) *
            1000000000 +
        ntohl((uint64_t)nanosecond);
    return t;
}

uint8_t temp[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08}; // 时钟标识

// 初始化PTP头
int init(PtpHeader *ptpheader, PtpMsgType type)
{
    // uint8_t src_mac[6] = {0xc8, 0x09, 0xa8, 0xe6, 0x9a, 0xc9}; // 设置源网卡地址
    // uint8_t dst_mac[6] = {0xc8, 0x09, 0xa8, 0xe6, 0x9a, 0xc9}; // 设置目的网卡地址

    // memcpy(ptpheader->dmac, dst_mac, sizeof(dst_mac));
    // memcpy(ptpheader->smac, src_mac, sizeof(src_mac));
    // ptpheader->eth_type = htons(0x88F7);
    // 测试数据，实际需要修改
    ptpheader->major_sdo_id = 0x1;
    ptpheader->message_type = static_cast<uint8_t>(type);
    ptpheader->version_ptp = 0x2;
    ptpheader->minor_version_ptp = 0x0;

    ptpheader->domain_number = 0xab; // 2
    ptpheader->minor_sdo_id = 0xcd;

    ptpheader->correction_field = 0x123456789ABCDEF0;
    ptpheader->message_type_specific = 0x12345678;
    switch (type)
    {
    case E_SIGNAL:
        ptpheader->message_length = PTP_SIGNAL_LENGTH;
        ptpheader->flags |= PTP_TIMESCALE;
        ptpheader->log_message_interval = 127;
        break;
    case E_SYNC:
        ptpheader->message_length = PTP_SYNC_LENGTH;
        ptpheader->flags |= PTP_TIMESCALE;
        ptpheader->flags |= TOW_STEP;
        ptpheader->control_field = 0;
        ptpheader->log_message_interval = 0;
        break;
    case E_PDELAY_REQ:
        ptpheader->message_length = PTP_PDELAY_REQ_LENGTH;
        // ptpheader->major_sdo_id = 2;
        ptpheader->control_field = 0x05;
        ptpheader->flags |= PTP_TIMESCALE;
        ptpheader->log_message_interval = 127;
        break;
    case E_PDELAY_RESP:
        ptpheader->message_length = PTP_PDELAY_RESP_LENGTH;
        // ptpheader->major_sdo_id = 2;
        ptpheader->control_field = 0x05;
        ptpheader->flags |= PTP_TIMESCALE;
        ptpheader->flags |= TOW_STEP;
        ptpheader->log_message_interval = 127;
        break;
    case E_PDELAY_RESP_FOLLOW_UP:
        ptpheader->message_length = PTP_PDELAY_RESP_FLLOW_UP_LENGTH;
        // ptpheader->major_sdo_id = 2;
        ptpheader->control_field = 0x05;
        ptpheader->flags |= PTP_TIMESCALE;
        ptpheader->log_message_interval = 127;
        break;
    case E_FOLLOW_UP:
        ptpheader->message_length = PTP_FOLLOW_UP_LENGTH;
        ptpheader->flags |= PTP_TIMESCALE;
        ptpheader->control_field = 0x02;
        ptpheader->log_message_interval = 127;
        break;
    case E_ANNOUNCE:
        ptpheader->message_length = PTP_FOLLOW_UP_LENGTH;
        ptpheader->control_field = 0x05;
        ptpheader->flags |= PTP_UTC_REASONABLE;
        ptpheader->log_message_interval = 127;
        break;
    default:
        return -1;
    }

    // ptpheader->message_length = 60;
    // ptpheader->flags = 0;
    memcpy(ptpheader->clock_identify, temp, sizeof(temp));
    ptpheader->port_number = 0x6060;
    ptpheader->sequence_id = port_delay.sequence_id;
    // ptpheader->control_field = 0x05;

    link_delay_state = Port_delay_state::NONE;

    return 0;
} // 发送Pdelay_req
uint8_t *sendPdelayReq()
{
    if (link_delay_state == Port_delay_state::DONE)
    {
        port_delay.last_timestamp_3 = port_delay.timestamp_3;
        port_delay.last_timestamp_4 = port_delay.timestamp_4;
    }
    PtpHeader pdelay_req_header;
    init(&pdelay_req_header, E_PDELAY_REQ); // 初始化PTP头
    PtpPdelayReq pdelay_req;
    memcpy(&pdelay_req.ptp_header, &pdelay_req_header, sizeof(pdelay_req_header));
    memset(pdelay_req.padding, 0, 20);
    memcpy(buffer_, &pdelay_req, sizeof(pdelay_req));
    link_delay_state = Port_delay_state::SEND_REQ;
    return buffer_;
}

// 发送Pdelay_resp
uint8_t *sendPdelayResp()
{
    PtpHeader resp_header;
    init(&resp_header, E_PDELAY_RESP);
    PtpPdelayResp resp;
    memcpy(&resp.ptp_header, &resp_header, sizeof(resp_header));
    uint8_t t2[10] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // 获取t2值，后续补充
    memcpy(&resp.request_receipt_timestamp, &t2, sizeof(t2));
    memcpy(&resp.clock_identify, &temp, sizeof(temp));
    resp.port_number = resp_header.port_number;
    memcpy(buffer_, &resp, sizeof(resp));
    return buffer_;
}

// 发送Pdelay_resp_follow_up
uint8_t *sendPdelayRespFollowUp()
{
    PtpHeader resp_header;
    init(&resp_header, E_PDELAY_RESP_FOLLOW_UP);
    PtpPdelayRespFollowUp respfollowup;
    memcpy(&respfollowup.ptp_header, &resp_header, sizeof(resp_header));
    uint8_t t3[10] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // 获取t3值，后续补充
    memcpy(&respfollowup.request_origin_timestamp, &t3, sizeof(t3));
    memcpy(&respfollowup.clock_identify, &temp, sizeof(temp));
    respfollowup.port_number = resp_header.port_number;
    memcpy(buffer_, &respfollowup, sizeof(respfollowup));
    return buffer_;
}

// 接收PdelayReq帧
int parsePdelayReq(struct Pack_rec tsmp_pack) // 需要tsmp
{
    struct PtpPdelayReq req;
    memcpy(&req, &tsmp_pack, sizeof(struct PtpPdelayReq));

    uint64_t t2 = ((uint64_t)tsmp_pack.head.local_time_sec << 32) | tsmp_pack.head.local_time_nanosec; // 获取t2时间戳

    port_delay.timestamp_2 = t2;                         // 写入t2
    port_delay.sequence_id = req.ptp_header.sequence_id; // 把序列号赋值，其余没有赋值，现阶段只是测试，所以序列号递增还未实现

    return 0;
}

// 接收PdelayResp帧
int parsePdelayResp(struct Init_pack init_pack) // 需要ptp
{
    if (link_delay_state != Port_delay_state::SEND_REQ) // 怀疑此处判断状态机有问题，不确定是否在requester.cpp中调用了sendPdelayReq函数后状态机的改变是否保存了。
    {
        std::cout << "recv resp pkt,state_ = " << (int)link_delay_state;
        return -1;
    }

    struct PtpPdelayResp resp;
    memcpy(&resp, &init_pack, sizeof(struct PtpPdelayResp));

    // uint64_t t2 = resp->request_receipt_timestamp;
    uint64_t t2 = BufferToTimeStamp(resp.request_receipt_timestamp); // 获取t3

    link_delay_state = Port_delay_state::RECV_RESP; // 填状态机
    port_delay.timestamp_2 = t2;                    // 写入t2
    return 0;
}

// 接收PdelayRespFollowUp帧
int parsePdelayRespFollowUp(struct Init_pack init_pack)
{
    if (link_delay_state != Port_delay_state::RECV_RESP)
    {
        std::cout << "recv resp pkt,state_ = " << (int)link_delay_state;
        return -1;
    }

    struct PtpPdelayRespFollowUp resp_follow_up;
    memcpy(&resp_follow_up, &init_pack, sizeof(struct PtpPdelayRespFollowUp));

    uint64_t t3 = BufferToTimeStamp(resp_follow_up.request_origin_timestamp); // 获取t4
    link_delay_state = Port_delay_state::RECV_RESP_FOLLOW_UP;
    port_delay.timestamp_3 = t3;
    // calculateLinkDelay();
    return 0;
}

// 计算链路延时
int calculateLinkDelay()
{
    if (link_delay_state != Port_delay_state::RECV_RESP_FOLLOW_UP)
    {
        return -1;
    }

    port_delay.link_delay = ((port_delay.timestamp_4 - port_delay.timestamp_1) - (port_delay.timestamp_3 - port_delay.timestamp_2)) / 2;
    // port_delay.link_delay = cur_link_delay;
    link_delay_state = Port_delay_state::DONE;
    calculateNR();
    return 0;
}

// 计算邻居频比
int calculateNR()
{
    if (port_delay.last_timestamp_3 == 0 || port_delay.last_timestamp_4 == 0)
    {
        return 0;
    }
    double cur_nr = (port_delay.timestamp_3 - port_delay.last_timestamp_3) / (1.0 * (port_delay.timestamp_4 - port_delay.last_timestamp_4));
    port_delay.neighbour_rateratio = cur_nr;
    return 0;
}

// 将本节点的GM频率比写进寄存器
int w_register(uint32_t addr)
{                                           // 获取csro,上一节点的GM频率比
    int32_t cumulativeScaledRateOffset = 1; // 测试
    // 获取邻居频比
    // double nr = port_delay.neighbour_rateratio;
    double nr = 1.252;
    double ticklen = 8 * (((double)cumulativeScaledRateOffset / pow(2, 41) + 1) * nr);
    // std::vectortor<uint32_t> vector; // 将64位ticklen转化为两个32位数组
    uint32_t vector[2];             // 寄存器高低位，总共8个字节，写两个值
    uint32_t high_part, low_part;   // 高低位
    uint32_t a = (uint32_t)ticklen; // 获取整数部分
    a = a << 9;                     // a向左移9位
    // 取得ticklen的整数部分
    double ticklen_integer_part = trunc(ticklen);
    // 取得ticklen的小数部分
    double ticklen_decimal_part = ticklen - ticklen_integer_part;
    uint64_t b = (uint64_t)(ticklen_decimal_part * pow(2, 41)); // 小数部分乘以2的41次方
    low_part = (uint32_t)b;                                     // 获取低32位
    b = b >> 32;                                                // 再往左移32位
    high_part = a | b;                                          // 将前32位进行操作
    printf("highpart:%d\n", high_part);
    printf("lowpart:%d\n", low_part);
    // vector.push_back(high_part);
    // vector.push_back(low_part);
    vector[0] = high_part;
    vector[1] = low_part;
    int iRet;
    // 写入寄存器
    iRet = writeReqSend(addr, 2, vector);
    if (iRet == -1)
    {
        std::cout << "write data error" << std::endl;
        return -1;
    }
    iRet = readReqSend(addr, 2);
    if (iRet == -1)
    {
        std::cout << "read data error" << std::endl;
        return -1;
    }
    return 0;
}